{{ Continous Servo Research Build }}
CON
        _clkmode = xtal1 + pll16x                                               
        _xinfreq = 5_000_000

  ServoCh1 = 16
  ServoCh2 = 17
  ServoCh3 = 18
  ServoCh4 = 19

  Forward  = 1
  Reverse  = 2
  Left     = 3
  Right    = 4
  Stop     = 5
  
OBJ
  pst : "Parallax Serial Terminal"
  servo : "Servo32v7"

PUB Main
  Initialize

  Drive(Stop) 
  
PUB Initialize
  servo.Start
  servo.Ramp

PUB Drive(Direction)             ' Syntax: Drive( Direction to Drive in)  Options: Forward, Reverse, Right, Left, Stop
  if (direction == Forward)
      servo.Set(ServoCh1,2000)
      servo.Set(ServoCh2,2000)
      servo.Set(ServoCh3,1000)
      servo.Set(ServoCh4,1000)    
  if (direction == Reverse)
      servo.Set(ServoCh1,1000)
      servo.Set(ServoCh2,1000)
      servo.Set(ServoCh3,2000)
      servo.Set(ServoCh4,2000)  
  if (direction == Right)
      servo.Set(ServoCh1,2000)
      servo.Set(ServoCh2,2000)
      servo.Set(ServoCh3,2000)
      servo.Set(ServoCh4,2000)
  if (direction == Left)
      servo.Set(ServoCh1,1000)
      servo.Set(ServoCh2,1000)
      servo.Set(ServoCh3,1000)
      servo.Set(ServoCh4,1000)
  if (direction == Stop)
      servo.Set(ServoCh1,1500)
      servo.Set(ServoCh2,1500)
      servo.Set(ServoCh3,1500)
      servo.Set(ServoCh4,1500)
      